#include "ModelImporterDialog.h"
#include "ui_ModelImporterDialog.h"

#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <VirtualRobot/Robot.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
#include <QSignalMapper>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <Inventor/actions/SoLineHighlightRenderAction.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/Qt/SoQt.h>

ModelImporterDialog::ModelImporterDialog(MMM::ModelPtr model, QWidget* parent) :
    QDialog(parent),
    ui(new Ui::ModelImporterDialog),
    model(model),
    robot(MMM::SimoxTools::buildModel(model))
{
    ui->setupUi(this);

    connect(ui->rootPositionX, SIGNAL(valueChanged(double)), this, SLOT(poseChanged()));
    connect(ui->rootPositionY, SIGNAL(valueChanged(double)), this, SLOT(poseChanged()));
    connect(ui->rootPositionZ, SIGNAL(valueChanged(double)), this, SLOT(poseChanged()));
    connect(ui->rootRotationX, SIGNAL(valueChanged(double)), this, SLOT(poseChanged()));
    connect(ui->rootRotationY, SIGNAL(valueChanged(double)), this, SLOT(poseChanged()));
    connect(ui->rootRotationZ, SIGNAL(valueChanged(double)), this, SLOT(poseChanged()));

    ui->motionName->setText(QString::fromStdString(model->getName()));

    if (MMM::XML::getFileName(model->getFilename()) == "mmm.xml") ui->rootPositionZ->setValue(540.0); // set model on top of floor

    connect(ui->importMotionButton, SIGNAL(clicked()), this, SLOT(importMotion()));
    connect(ui->cancelButton, SIGNAL(clicked()), this, SLOT(close()));

    QSignalMapper* signalMapper = new QSignalMapper(this);
    ui->treeWidget->setHeaderLabels({"Joint name", "Joint angle value"});
    int index = 0;
    for (VirtualRobot::RobotNodePtr robotNode : robot->getRobotNodes()) {
        QDoubleSpinBox* spin = new QDoubleSpinBox();
        spin->setValue(robotNode->getJointValue());
        spin->setMinimum(robotNode->getJointLimitLow());
        spin->setDecimals(4);
        spin->setSingleStep(0.1);
        spin->setMaximum(robotNode->getJointLimitHigh());
        spin->setToolTip(QString::fromStdString(std::to_string(robotNode->getJointLimitLow()) + " to " + std::to_string(robotNode->getJointLimitHigh())));
        QTreeWidgetItem* item = new QTreeWidgetItem();
        item->setData(0, Qt::DisplayRole, QString::fromStdString(robotNode->getName()));
        ui->treeWidget->insertTopLevelItem(index, item);
        ui->treeWidget->setItemWidget(item, 1, spin);
        connect(spin, SIGNAL(valueChanged(double)), signalMapper, SLOT(map()));
        signalMapper->setMapping(spin, (QWidget*) item);
        index++;
    }
    connect(signalMapper, SIGNAL(mapped(QWidget*)), this, SLOT(jointAngleChanged(QWidget*)));
    ui->treeWidget->resizeColumnToContents(0);

    setupViewer();
}

ModelImporterDialog::~ModelImporterDialog() {
    delete ui;
}

void ModelImporterDialog::jointAngleChanged(QWidget* widget) {
    std::string jointName = ((QTreeWidgetItem*) widget)->data(0, Qt::DisplayRole).toString().toStdString();
    float jointValue = ((QDoubleSpinBox*) ui->treeWidget->itemWidget((QTreeWidgetItem*) widget, 1))->value();
    robot->setJointValue(robot->getRobotNode(jointName), jointValue);
    if (jointValue == 0) {
        if (jointValues.find(jointName) != jointValues.end())
            jointValues.erase(jointValues.find(jointName));
    }
    else jointValues[jointName] = jointValue;
}

void ModelImporterDialog::poseChanged() {
    robot->setGlobalPose(MMM::Math::poseRPYToMatrix4f(Eigen::Vector3f(ui->rootPositionX->value(),ui->rootPositionY->value(),ui->rootPositionZ->value()), Eigen::Vector3f(ui->rootRotationX->value(),ui->rootRotationY->value(),ui->rootRotationZ->value())));
}

void ModelImporterDialog::importMotion() {
    MMM::MotionPtr motion(new MMM::Motion(ui->motionName->text().toStdString(), model, model));

    MMM::ModelPoseSensorPtr modelPoseSensor(new MMM::ModelPoseSensor());
    MMM::ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement(new MMM::ModelPoseSensorMeasurement(0.0f, Eigen::Vector3f(ui->rootPositionX->value(),ui->rootPositionY->value(),ui->rootPositionZ->value()), Eigen::Vector3f(ui->rootRotationX->value(),ui->rootRotationY->value(),ui->rootRotationZ->value()), MMM::SensorMeasurementType::EXTENDED));
    modelPoseSensor->addSensorMeasurement(modelPoseSensorMeasurement);
    motion->addSensor(modelPoseSensor);

    std::vector<std::string> jointNames;
    Eigen::VectorXf jointAngles(jointValues.size());
    int index = 0;
    for (const auto &j : jointValues) {
        jointNames.push_back(j.first);
        jointAngles(index) = j.second;
        index++;
    }
    MMM::KinematicSensorPtr kinematicSensor(new MMM::KinematicSensor(jointNames));
    MMM::KinematicSensorMeasurementPtr kinematicSensorMeasurement(new MMM::KinematicSensorMeasurement(0.0f, jointAngles, MMM::SensorMeasurementType::EXTENDED));
    kinematicSensor->addSensorMeasurement(kinematicSensorMeasurement);
    motion->addSensor(kinematicSensor);

    motions.push_back(motion);
    close();
}

MMM::MotionList ModelImporterDialog::getMotions() {
    exec();
    return motions;
}

void ModelImporterDialog::setupViewer() {
    SoSeparator* sceneGraph = new SoSeparator();
    sceneGraph->ref();
    sceneGraph->addChild(VirtualRobot::CoinVisualizationFactory::CreatePlaneVisualization(Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 1.0f), 10000.0f, 0));
    SoUnits* u = new SoUnits();
    u->units = SoUnits::MILLIMETERS;
    sceneGraph->addChild(u);

    viewer = new SoQtExaminerViewer(ui->viewer, "", TRUE, SoQtExaminerViewer::BUILD_POPUP);
    viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
    viewer->setAccumulationBuffer(true);
    viewer->setGLRenderAction(new SoLineHighlightRenderAction);
    viewer->setTransparencyType(SoGLRenderAction::BLEND);
    viewer->setFeedbackVisibility(true);
    viewer->setSceneGraph(sceneGraph);
    viewer->viewAll();

    MMM::SimoxTools::updateInertialMatricesFromModels(robot);
    VirtualRobot::CoinVisualizationPtr visualization; visualization = robot->getVisualization<VirtualRobot::CoinVisualization>(VirtualRobot::SceneObject::Full);
    SoNode* visualisationNode = visualization->getCoinVisualization();
    if (visualisationNode) sceneGraph->addChild(visualisationNode);

    setupCamera();
}

void ModelImporterDialog::setupCamera() {
    QSettings settings;
    qRegisterMetaTypeStreamOperators<QList<float> >("QList<float>");
    // The following copied from MMMViewer
    SoCamera* camera = viewer->getCamera();

    QList<float> qInitPosition;
    qInitPosition.append(-4.0f);
    qInitPosition.append(0.0f);
    qInitPosition.append(1.0f);
    QList<float> qPosition = settings.value("camera/position", QVariant::fromValue(qInitPosition)).value<QList<float> >();
    camera->position.setValue(SbVec3f(qPosition[0], qPosition[1], qPosition[2]));

    QList<float> qInitOrientation;
    qInitOrientation.append(0.5f);
    qInitOrientation.append(-0.5f);
    qInitOrientation.append(-0.5f);
    qInitOrientation.append(0.5f);
    QList<float> qOrientation = settings.value("camera/orientation", QVariant::fromValue(qInitOrientation)).value<QList<float> >();
    camera->orientation.setValue(SbRotation(qOrientation[0], qOrientation[1], qOrientation[2], qOrientation[3]));

    camera->nearDistance.setValue(settings.value("camera/nearDistance", 0.0185f).toFloat());
    camera->farDistance.setValue(settings.value("camera/farDistance", 9.5f).toFloat());
    camera->focalDistance.setValue(settings.value("camera/focalDistance", 4.5f).toFloat());
}
